package org.electrode.system;

import org.electrode.base.XboxJoystick;
import org.electrode.common.RobotSystem;
import org.electrode.common.NetworkTableVariable;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.tables.ITable;

public class Camera extends RobotSystem {
	private static final double CHANGE_AMOUNT = 0.01;
	private final XboxJoystick control;
	private Servo xServo;
	private Servo yServo;
	private ITable table;
	private NetworkTableVariable x;
	private NetworkTableVariable y;

	public Camera(XboxJoystick joystick) {
		control = joystick;
	}

	public void init() {
		xServo = new Servo(3);
		yServo = new Servo(4);
		table = NetworkTable.getTable("Camera");

		xServo.initTable(table.getSubTable("xServo"));

		yServo.initTable(table.getSubTable("yServo"));
		x = new NetworkTableVariable(table, "X", 0.394);
		y = new NetworkTableVariable(table, "Y", 0.480);
	}

	public void periodicTeleop() {
		xServo.updateTable();
		yServo.updateTable();

		double dx = control.getRightX();
		double dy = control.getRightY();
		table.putNumber("X", dx);
		table.putNumber("Y", dy);

		x.set(x.get() + (dx * CHANGE_AMOUNT));
		y.set(y.get() + (dy * CHANGE_AMOUNT));
		xServo.set(x.get());
		yServo.set(y.get());
	}

	public String getName() {
		return "Camera";
	}
	/**
	 * Legacy upDown += shooterControl.getRightX() / 50; if (upDown > 1) upDown
	 * = 1; else if (upDown < 0) upDown = 0; leftRight +=
	 * shooterControl.getRightY() / 50; if (leftRight > 1) leftRight = 1; else
	 * if (leftRight < 0) leftRight = 0; cameraUpDown.set(upDown);
	 * cameraLeftRight.set(leftRight);
	 */

}
